#pragma once

#include "rclcpp/rclcpp.hpp"
#include "io_macro.h"
// 模拟量通道结构参数
struct AnalogChannel
{
    // 归一化数值范围:y ;norm_min<norm_max 或 可设置 norm_max<norm_min
    double norm_min;
    double norm_max;
    // 原始数据范围:x
    // y = kx +b; raw_data_min <raw_data_max
    int raw_data_min;
    int raw_data_max;
    AnalogChannel()
    {
        norm_min = 0.0;
        norm_max = 0.0;
        raw_data_min = 0;
        raw_data_max = 0;
        k = 0;
        b = 0;
    }

    bool toNorm(const uint16_t &raw, double &norm)
    {
        bool ret = false;
        if (solve())
        {
            norm = k * raw + b;
            return validNorm(norm);
        }

        return ret;
    }

    bool toRawData(const double &norm, uint16_t &raw)
    {
        bool ret = false;
        if (solve())
        {
            double x = (norm - b) / k;
            if (x >= 0)
            {
                raw = x;
                return validRaw(raw);
            }
            DEBUG_STREAM_OUT("error:toRawData rawData(<0)=" << x);
            return false;
        }
        return ret;
    }

private:
    // 求解方程系数 y=kx+b
    //  y:norm x:raw_data
    double k; // 可正 可负
    double b;
    bool solve()
    {
        int diff = raw_data_max - raw_data_min;
        if (diff <= 0)
        {
            DEBUG_ERROR_OUT("error:AnalogChannel sove failed;"
                             << "raw:max=" << raw_data_max
                             << ",min=" << raw_data_min
                             << ",norm:max=" << norm_max
                             << ",min=" << norm_min

            );
            return false;
        }

        k = (norm_max - norm_min) / diff;
        b = norm_min - k * raw_data_min;
        return true;
    }

    bool validRaw(const uint16_t &raw)
    {
        if (raw < raw_data_min || raw > raw_data_max)
        {
            DEBUG_ERROR_OUT("error:validRaw failed;raw=" << raw << ",min=" << raw_data_min
                                                          << ",max=" << raw_data_max);
            return false;
        }
        return true;
    }

    bool validNorm(const double &norm)
    {
        double max = norm_max > norm_min ? norm_max : norm_min;
        double min = norm_max < norm_min ? norm_max : norm_min;
        if (norm < min || norm > max)
        {
            DEBUG_ERROR_OUT("error:validNorm failed;norm=" << norm << ",min=" << norm_min
                                                            << ",max=" << norm_max);
            return false;
        }
        return true;
    }
};

struct IoGroup
{
    // 输入输出数量
    int num_input;
    int num_output;
    // 模拟量输入输出数量
    int num_analog_input;
    int num_analog_output;
    // 输入数量+输出数量+极性
    std::vector<bool> polarity_input;
    std::vector<bool> polarity_output;
    // 输入输出模拟量通道管理
    std::vector<AnalogChannel> analog_input_device;
    std::vector<AnalogChannel> analog_output_device;
    IoGroup()
    {
        num_input = 0;
        num_output = 0;
        num_analog_input = 0;
        num_analog_output = 0;
    }

    void resize()
    {
        if (num_input < 0 || num_output < 0 || num_analog_input < 0 || num_analog_output < 0)
        {
            DEBUG_ERROR_OUT("io  module is error:" << ",num_input=" << num_input
                                                    << ",num_output=" << num_output
                                                    << ",num_analog_input=" << num_analog_input
                                                    << ",num_analog_output=" << num_analog_output);
            return;
        }

        polarity_input.resize(num_input);
        polarity_output.resize(num_output);

        analog_input_device.resize(num_analog_input);
        analog_output_device.resize(num_analog_output);

        DEBUG_STREAM_OUT("num_input=" << num_input
                                     << ",num_output=" << num_output
                                     << ",num_analog_input=" << num_analog_input
                                     << ",num_analog_output=" << num_analog_output);
    }
};

struct AgvStateInfo
{
    bool init; // 是否初始化了
    // agvinfo 具体信息字段 暂时只留4、22、23位
    uint64_t agvInfo;
    bool manul; // 4
    //....
    bool halfAuto;   // 22
    bool autoOnline; // 23
    AgvStateInfo()
    {
        agvInfo = 0;
        manul = false;
        halfAuto = false;
        autoOnline = false;
        init = false;
    }
};

struct Lamp
{
    // 三色灯是否开启/关闭
    using MapLighOutType = std::map<std::string, bool>;
    MapLighOutType color;
    bool set_frequency_;
    bool level_;
    int on_time_;
    int off_time_;

    // 当前灯光使用场景
    int cur_scene;
    // 上一次灯光使用的场景
    int last_scene;
    Lamp()
    {
        color = {
            {"green", false},
            {"yellow", false},
            {"red", false}};

        set_frequency_ = false;
        level_ = false;
        on_time_ = 0;
        off_time_ = 0;

        cur_scene = last_scene = 0;
    }
};

/////////IoList使用///////////

struct SetFrequency
{
    // 名字
    std::string name;
    // io索引
    int32_t index;
    // 设置电平
    bool level;
    // 是否开启频率
    bool frequency;
    // 打开/高电平时间ms
    int32_t on_time;
    // 关闭/低电平时间ms
    int32_t off_time;
    SetFrequency()
    {
        clear();
    }
    void clear()
    {
        name = "";
        index = 0;
        level = false;
        frequency = false;
        on_time = 0;
        off_time = 0;
    }
};

// 强行设置一次blink
class BlinkTrigger
{
public:
    explicit BlinkTrigger(const std::shared_ptr<rclcpp::Node>& node)
        : node_(node), trigTime(node->now()) {}

    void triggerOnce()
    {
        trigTime = node_->now();
        DEBUG_OUT("BlinkTrigger trig once");
    }

    bool trigState()
    {
        bool ret = false;
        rclcpp::Time curTime = node_->now();
        const auto duration = curTime - trigTime;
        return duration.seconds() < 3.0;
    }

private:
    rclcpp::Time trigTime;
    std::shared_ptr<rclcpp::Node> node_;
};
